using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Globalization;

namespace Dida
{
	/// <summary>
	/// The enumeration of units that can be used the specify angles.
	/// </summary>
	public enum AngleUnit
	{
		/// <summary>
		/// A rotation in radians, which means a whole circle has an angle of 
		/// 2 * PI.
		/// </summary>
		/// <remarks>
		/// This is the default format for angles in Dida. If not specified
		/// differently, all angles will be in radians.
		/// </remarks>
		Radians,
		/// <summary>
		/// A rotation in degrees, which means a whole circle has an angle
		/// of 360.
		/// </summary>
		Degrees,

		/// <summary>
		/// Unreal rotation units. A whole circle has an angle of 65536. Very
		/// cool for doing bitmasking tricks.
		/// </summary>
		[Obsolete("It's not 1982 any more.",true)]
		URUs,
	}

	/// <summary>
	/// A static class with functionality for working with rotations.
	/// </summary>
	public static class Rotation
	{
		/// <summary>
		/// Converts the given angle from degrees to radians.
		/// </summary>
		/// <param name="angle">An angle, in degrees.</param>
		/// <returns>The angle in radians that specifies the same
		/// rotation as the given parameter.</returns>
		public static float Deg2Rad(float angle)
		{
			return angle * (float)Math.PI / 180;
		}

		/// <summary>
		/// Converts the given angle from radians to degrees.
		/// </summary>
		/// <param name="angle">An angle, in radians.</param>
		/// <returns>The angle in degrees that specifies the same
		/// rotation as the given parameter.</returns>
		public static float Rad2Deg(float angle)
		{
			return angle * 180 / (float)Math.PI;
		}

		/// <summary>
		/// Converts the given angle to an angle
		/// that represent the same rotation, but is always in the
		/// -PI - PI range (would be -180 - 180 in degrees).
		/// </summary>
		/// <param name="angle">The input angle, in radians.</param>
		/// <returns>The normalized angle.</returns>
		public static float GetNormalized(float angle)
		{
			if (angle > 0)
			{
				float div = (angle + (float)Math.PI) / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				return angle - (div * (2 * (float)Math.PI));
			}
			else
			{
				angle = -angle;

				float div = (angle + (float)Math.PI) / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				return -(angle - (div * (2 * (float)Math.PI)));
			}
		}

		public static void Normalize(ref float angle)
		{
			if (angle > 0)
			{
				float div = (angle + (float)Math.PI) / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				angle = angle - (div * (2 * (float)Math.PI));
			}
			else
			{
				angle = -angle;

				float div = (angle + (float)Math.PI) / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				angle = -(angle - (div * (2 * (float)Math.PI)));
			}
		}

		/// <summary>
		/// Converts the given angle to an angle
		/// that represent the same rotation, but is always in the
		/// 0 - 2 * PI range (would be 0 - 360 in degrees).
		/// </summary>
		/// <param name="angle">The input angle, in radians.</param>
		/// <returns>The normalized angle.</returns>
		public static float GetNormalizedPositive(float angle)
		{
			if (angle > 0)
			{
				float div = angle / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				return angle - (div * (2 * (float)Math.PI));
			}
			else
			{
				angle = -angle;

				float div = angle / (2 * (float)Math.PI);
				div = (float)Math.Truncate(div);
				return (2 * (float)Math.PI) - (angle - (div * (2 * (float)Math.PI)));
			}	
		}		
	}

	/// <summary>
	/// An EulerRot represents a rotation, using 3 rotations about the principle axes,
	/// that are applied after each other. For objects, the rotations are
	/// applied in the roll -> pitch -> yaw order. For cameras, the angles
	/// are negated, and applied in the yaw -> pitch -> roll order.
	/// <list>
	/// <item>The yaw rotation is about the y axis. If looked at it in the positive y direction,
	/// the rotation is in counter clockwise direction.</item>
	/// <item>The pitch rotation is about the x axis. If looked at it in the positive x direction,
	/// the rotation is in counter clockwise direction.</item>
	/// <item>The roll rotation is about the z axis. If looked at it in the positive z direcion,
	/// the rotation is in counter clockwise direction.</item>
	/// </list>
	/// The angles are in radians.
	/// </summary>
	[StructLayout(LayoutKind.Sequential)]
	[Dxml.Parsable]
	public struct EulerRot
	{
		/// <summary>
		/// The yaw angle, in radians.
		/// </summary>
		public float yaw;
		/// <summary>
		/// The pitch angle, in radians.
		/// </summary>
		public float pitch;
		/// <summary>
		/// The roll angle, in radians.
		/// </summary>
		public float roll;

		/// <summary>
		/// Indexes the components of this EulerRot using an array indexer, where
		/// index 0 = yaw, index 1 = pitch and index 2 = roll.
		/// Any other index is invalid.
		/// </summary>
		public float this[int i]
		{
			get
			{
#if SECURE
				if (i == 0)
					return yaw;
				else if (i == 1)
					return pitch;
				else
					return roll;
#else
#if PROTECTED
				if (i < 0 || i > 2)
					throw new IndexOutOfRangeException();					
#endif
				unsafe
				{
					fixed (EulerRot* pThis = &this)
					{
						float* m = (float*)pThis;
						return m[i];
					}
				}
#endif
			}

			set
			{
#if SECURE
				if (i == 0)
					yaw = value;
				else if (i == 1)
					pitch = value;
				else
					roll = value;
#else
#if PROTECTED
				if (i < 0 || i > 2)
					throw new IndexOutOfRangeException();
#endif

				unsafe
				{
					fixed (EulerRot* pThis = &this)
					{
						float* m = (float*)pThis;
						m[i] = value;
					}
				}
#endif
			}
		}

		/// <summary>
		/// Creates an euler rot, initialized with the given angles.
		/// </summary>
		/// <param name="yaw">The yaw angle</param>
		/// <param name="pitch">The pitch angle</param>
		/// <param name="roll">The roll angle</param>
		public EulerRot(float yaw,float pitch,float roll)
		{
			this.yaw = yaw;
			this.pitch = pitch;
			this.roll = roll;
		}

		/// <summary>
		/// Creates an euler rot, initialized with the values in the 
		/// components array.
		/// </summary>
		/// <param name="components">An array containing 3 floats, 
		/// one for each component of the EulerRot</param>
		/// <see cref="EulerRot(float,float,float)"/>
		public EulerRot(float[] components)
		{
			yaw = components[0];
			pitch = components[1];
			roll = components[2];
		}

		/// <summary>
		/// Converts all angles in this EulerRot to angles
		/// that represent the same rotation, but are always in the
		/// -PI - PI range (would be -180 - 180 in degrees)
		/// </summary>
		public void Normalize()
		{
			yaw = (float)Math.IEEERemainder(yaw + (float)Math.PI,
				2 * (float)Math.PI) - (float)Math.PI;
			pitch = (float)Math.IEEERemainder(pitch + (float)Math.PI,
				2 * (float)Math.PI) - (float)Math.PI;
			roll = (float)Math.IEEERemainder(roll + (float)Math.PI,
				2 * (float)Math.PI) - (float)Math.PI;
		}

		/// <summary>
		/// Gets a <see cref = "Matrix4">Matrix4</see> that contains
		/// the rotations of this EulerRot.
		/// </summary>
		/// <seealso cref="InverseMatrix"/>
		public Matrix4 Matrix
		{
			get
			{
				return Matrix4.GetRollMatrix(roll) *
					Matrix4.GetPitchMatrix(pitch) *
					Matrix4.GetYawMatrix(yaw);
			}
		}

		/// <summary>
		/// Gets a <see cref = "Matrix4">Matrix4</see> that contains
		/// the inverse of the rotations of this EulerRot.
		/// </summary>
		/// <seealso cref="Matrix"/>
		public Matrix4 InverseMatrix
		{
			get
			{
				return Matrix4.GetYawMatrix(-yaw) *
					Matrix4.GetPitchMatrix(-pitch) *
					Matrix4.GetRollMatrix(-roll);
			}
		}

		/// <summary>
		/// Returns a <see cref="Quaternion">Quaternion</see> that 
		/// represents the same rotation as this EulerRot.
		/// </summary>
		/// <returns>The <see cref = "Quaternion">Quaternion</see>
		/// containing the rotation.</returns>
		public Quaternion ToQuaternion()
		{
			return Quaternion.RollQuaternion(roll) *
				Quaternion.PitchQuaternion(pitch) *
				Quaternion.YawQuaternion(yaw);
		}

		/// <summary>
		/// <para>
		/// Returns a string that contains the three angles of this rotation,
		/// separated by commas. The string is prefixed with (deg) to make
		/// clear that the format is in degrees.
		/// </para>
		/// <para>
		/// The returned string contains the angle in degrees. This is not the
		/// format that's used internally, but it's more human friendly in most 
		/// cases. If you want to specify the output unit yourself, 
		/// use the <see cref = "ToString(AngleUnit)">
		/// ToString(RotationFormat)</see> method.
		/// </para>
		/// </summary>
		/// <returns>A string respresenting this rotation</returns>
		public override string ToString()
		{
			return "(deg)" +
				Rotation.Rad2Deg(yaw).ToString() + ", " +
				Rotation.Rad2Deg(pitch).ToString() + ", " +
				Rotation.Rad2Deg(roll).ToString();
		}

		/// <summary>
		/// Returns a string that represents this rotation,
		/// using the given <see cref = "AngleUnit">RotationFormat</see>.
		/// The string is prefixed with the rotation format enclosed in parentheses.
		/// An example: <c>(deg)90,30,45</c>
		/// </summary>
		/// <param name="rotationFormat">The rotation format
		/// used in the string representation of this rotation.</param>
		/// <returns>The string that represents this rotation.</returns>
		/// <seealso cref="ToString()"/>
		/// <seealso cref="Parse"/>
		public string ToString(AngleUnit rotationFormat)
		{
			string ret = "";
			float conversionFactor = 0;
			switch (rotationFormat)
			{
				case AngleUnit.Radians:
					ret += "(rad)";
					conversionFactor = 1;
					break;
				case AngleUnit.Degrees:
					ret += "(deg)";
					conversionFactor = 180 / (float)Math.PI;
					break;

				default:
					Debug.Fail("Unknown RotationFormat");
					break;
			}

			ret += (yaw * conversionFactor).ToString() + ", " +
				(pitch * conversionFactor).ToString() + ", " +
				(roll * conversionFactor).ToString();
			return ret;
		}

		[Dxml.ParseMethod]
		[ParseMethod]
		public static EulerRot Parse(string s)
		{
			s = s.Trim();

			float conversionFactor = 1;
			if (s.StartsWith("(deg)"))
			{
				// degrees
				conversionFactor = (float)Math.PI / 180;
				s = s.Substring(5);
			}
			else if (s.StartsWith("(rad)"))
			{
				// radians
				conversionFactor = 1;
				s = s.Substring(5);
			}
			else
			{
				// default to degrees
				conversionFactor = (float)Math.PI / 180;
			}

			string[] components = s.Split(",".ToCharArray());
			if (components.Length != 3)
				throw new FormatException("An euler rotation should contain 3 components");

			EulerRot ret = new EulerRot();
			ret.yaw = float.Parse(components[0], CultureInfo.InvariantCulture) * conversionFactor;
			ret.pitch = float.Parse(components[1], CultureInfo.InvariantCulture) * conversionFactor;
			ret.roll = float.Parse(components[2], CultureInfo.InvariantCulture) * conversionFactor;
			return ret;
		}

		public Vector3D GetRotatedVector(Vector3D v)
		{
			float yawS = (float)Math.Sin(yaw);
			float yawC = (float)Math.Cos(yaw);

			float pitchS = (float)Math.Sin(pitch);
			float pitchC = (float)Math.Cos(pitch);

			float rollS = (float)Math.Sin(roll);
			float rollC = (float)Math.Cos(roll);

			Vector3D ret = new Vector3D();
			ret.x =
				(yawC * rollC + yawS * pitchS * rollS) * v.x +
				(yawS * pitchS * rollC - yawC * rollS) * v.y -
				yawS * pitchC * v.z;
			ret.y = pitchC * rollS * v.x + pitchC * rollC * v.y + pitchS * v.z;
			ret.z =
				(yawS * rollC - yawC * pitchS * rollS) * v.x -
				(yawS * rollS + yawC * pitchS * rollC) * v.y +
				yawC * pitchC * v.z;

			return ret;
		}

		public void RotateVector(ref Vector3D v)
		{
			v = GetRotatedVector(v);
		}
	}

	/// <summary>
	/// A quaternion.
	/// </summary>
	[StructLayout(LayoutKind.Sequential)]
	[Dxml.Parsable]
	public struct Quaternion
	{
		/// <summary>
		/// The real part.
		/// </summary>
		public float real;
		/// <summary>
		/// The imaginary vector.
		/// </summary>
		public Vector3D imaginary;

		/// <summary>
		/// Creates a quaternion, and initializes it with the given values
		/// </summary>
		/// <remarks>
		/// The values specify the quaternion in the following way:
		/// <code>
		/// quaternion = real + imaginary.x * <i>i</i> + imaginary.y * <i>j</i> + imaginary.z * <i>k</i>
		/// </code>
		/// </remarks>
		/// <param name="real">The real part of the quaternion</param>
		/// <param name="imaginary">The imaginary part of the quaternion</param>
		public Quaternion(float real, Vector3D imaginary)
		{
			this.real = real;
			this.imaginary = imaginary;
		}

		public Quaternion(float w, float x, float y, float z)
		{
			real = w;
			imaginary.x = x;
			imaginary.y = y;
			imaginary.z = z;
		}

		/// <summary>
		/// Creates a quaternion that represents the same rotation as the given
		/// <see cref = "EulerRot">EulerRot</see>.
		/// </summary>
		/// <param name="rot">The <see cref = "EulerRot">EulerRot</see>
		/// with the rotation you want to set this quaternion to.</param>
		public Quaternion(EulerRot rot)
		{
			this = Quaternion.RollQuaternion(rot.roll) *
				Quaternion.PitchQuaternion(rot.pitch) *
				Quaternion.YawQuaternion(rot.yaw);
		}

		/// <summary>
		/// Creates a quaternion from a rotation axis specified by a vector,
		/// and an angle which specifies the rotation about that axis.
		/// </summary>
		/// <param name="axis">The axis vector. This vector must 
		/// have a length of 1.</param>
		/// <param name="angle">The angle, in radians.</param>
		/// <returns>A quaternion that contains the rotation specified
		/// by the given axis and angle.</returns>
		public static Quaternion FromAxisAngle(Vector3D axis, float angle)
		{
			Quaternion ret = new Quaternion();

			float halfAngle = angle * .5f;

			float s = (float)Math.Sin(halfAngle);
			float c = (float)Math.Cos(halfAngle);

			ret.real = c;
			ret.imaginary = axis * s;
			
			return ret;
		}

		/// <summary>
		/// Returns a quaternion that specifies the given yaw rotation.
		/// </summary>
		/// <param name="angle">The angle, in radians</param>
		/// <returns>The quaternion.</returns>
		public static Quaternion YawQuaternion(float angle)
		{
			Quaternion ret = new Quaternion();

			float halfAngle = angle * .5f;
			ret.real = (float)Math.Cos(halfAngle);
			ret.imaginary = new Vector3D(0, (float)Math.Sin(halfAngle), 0);
			return ret;
		}

		/// <summary>
		/// Returns a quaternion that specifies the given pitch rotation.
		/// </summary>
		/// <param name="angle">The angle, in radians</param>
		/// <returns>The quaternion.</returns>
		public static Quaternion PitchQuaternion(float angle)
		{
			Quaternion ret = new Quaternion();

			float halfAngle = angle * .5f;
			ret.real = (float)Math.Cos(halfAngle);
			ret.imaginary = new Vector3D((float)Math.Sin(halfAngle), 0,0);
			return ret;
		}

		/// <summary>
		/// Returns a quaternion that specifies the given roll rotation.
		/// </summary>
		/// <param name="angle">The angle, in radians</param>
		/// <returns>The quaternion.</returns>
		public static Quaternion RollQuaternion(float angle)
		{
			Quaternion ret = new Quaternion();

			float halfAngle = -angle * .5f;
			ret.real = (float)Math.Cos(halfAngle);
			ret.imaginary = new Vector3D(0,0,(float)Math.Sin(halfAngle));
			return ret;
		}

		/// <summary>
		/// Returns the multiplicative identity quaternion. 
		/// The multiplicative identity quaternion is the quaternion
		/// that represents no rotation at all (like a rotation of an angle of 0).
		/// </summary>
		public static Quaternion Identity
		{
			get
			{
				return new Quaternion(1, 0, 0, 0);
			}
		}

		/// <summary>
		/// Gets or sets the given component of this quaternion.
		/// The indices correspond to the components in the following way:
		/// 0 = w, 1 = x, 2 = y, 3 = z.
		/// </summary>
		/// <param name="i">The index of the component</param>
		public float this[int i]
		{
			get
			{
				Debug.Assert(i >= 0 && i < 4);

#if SECURE
				if(i < 2)
				{
					if(i == 1)
						return real;
					else
						return imaginary.x;
				}
				else
				{
					if(i == 2)
						return imaginary.y;
					else
						return imaginary.z;
				}
#else
#if PROTECTED
				if (i < 0 || i > 3)
					throw new IndexOutOfRangeException();
#endif

				unsafe
				{
					fixed (Quaternion* p = &this)
					{
						return ((float*)p)[i];
					}
				}
#endif
			}

			set
			{
				Debug.Assert(i >= 0 && i < 4);

#if SECURE
				if(i < 2)
				{
					if(i == 1)
						real = value;
					else
						imaginary.x = value;
				}
				else
				{
					if(i == 2)
						imaginary.y = value;
					else
						imaginary.z = value;
				}
#else
#if PROTECTED
				if (i < 0 || i > 3)
					throw new IndexOutOfRangeException();
#endif
				unsafe
				{
					fixed(Quaternion *p = &this)
					{
						((float*)p)[i] = value;
					}
				}
#endif
			}
		}

		public static Quaternion operator + (Quaternion a,Quaternion b)
		{
			return new Quaternion(
				a.real + b.real,
				a.imaginary.x + b.imaginary.x,
				a.imaginary.y + b.imaginary.y,
				a.imaginary.z + b.imaginary.z);
		}

		public static Quaternion operator -(Quaternion a, Quaternion b)
		{
			return new Quaternion(
				a.real - b.real,
				a.imaginary.x - b.imaginary.x,
				a.imaginary.y - b.imaginary.y,
				a.imaginary.z - b.imaginary.z);
		}

		/// <summary>
		/// Multiplies two quaternions
		/// </summary>
		/// <param name="a">The first operand.</param>
		/// <param name="b">The second operand.</param>
		/// <returns>The result of the multiply operations</returns>
		/// <remarks>
		/// Mulitplying two quaternions gives you a quaternion
		/// that represents the combined rotation, of quaternion
		/// <paramref name="a"/>, followed by the rotation of 
		/// quaternion <paramref name="b"/>
		/// </remarks>
		public static Quaternion operator *(Quaternion a, Quaternion b)
		{
			Quaternion ret = new Quaternion();
			ret.real = a.real * b.real - Vector3D.DotProduct(
				a.imaginary, b.imaginary);
			ret.imaginary = a.imaginary * b.real + b.imaginary * a.real +
				Vector3D.CrossProduct(a.imaginary, b.imaginary);
			return ret;
		}

		public static Quaternion operator *(Quaternion a, float b)
		{
			return new Quaternion(a.real * b, a.imaginary * b);
		}

		public static Quaternion operator -(Quaternion a)
		{
			return new Quaternion(-a.real, -a.imaginary);
		}

		/// <summary>
		/// Returns the conjugate of this quaternion.
		/// </summary>
		/// <returns>The conjugate.</returns>
		public Quaternion GetConjugate()
		{
			return new Quaternion(real, -imaginary);
		}

		/// <summary>
		/// Gets the norm (length) of this quaternion. Note that
		/// to specify a rotation with a quaternion, it needs to have
		/// a norm of 1, so for most quaternions this will be 1.
		/// </summary>
		/// <seealso cref="SqrNorm"/>
		/// <seealso cref="Normalize"/>
		public float Norm
		{
			get
			{
				return (float)Math.Sqrt(real * real + imaginary.x * imaginary.x +
					imaginary.y * imaginary.y + imaginary.z * imaginary.z);
			}
		}

		/// <summary>
		/// Gets the squared norm (squared length) of this quaternion.
		/// Getting the SqrNorm is faster than getting the <see cref = "Norm">Norm</see>,
		/// so to optimize your code, you should prefer this property
		/// over the <see cref = "Norm">Norm</see> property.
		/// </summary>
		/// <seealso cref="Norm"/>
		/// <seealso cref="Normalize"/>
		public float SqrNorm
		{
			get
			{
				return real * real + imaginary.x * imaginary.x +
					imaginary.y * imaginary.y + imaginary.z * imaginary.z;
			}
		}

		/// <summary>
		/// Scales this quaternion so that it becomes a unit quaternion
		/// (a quaternion with <see cref = "Norm">Norm</see> 1).
		/// </summary>
		public void Normalize()
		{
			float scale = 1 / (float)Math.Sqrt(real * real + imaginary.x *
				imaginary.x + imaginary.y * imaginary.y + imaginary.z * imaginary.z);
			real *= scale;
			imaginary.x *= scale;
			imaginary.y *= scale;
			imaginary.z *= scale;
		}
		
		/// <summary>
		/// Creates a matrix that contains the rotation specifyed by 
		/// this quaternion.
		/// </summary>
		/// <returns>The newly creates matrix.</returns>
		/// <seealso cref="GetInverseMatrix"/>
		public Matrix4 ToMatrix()
		{
			Matrix4 ret = new Matrix4();
			ret.rows[3].w = 1;

			Quaternion tmp = new Quaternion(0, new Vector3D(1, 0, 0));
			tmp = this * tmp * GetConjugate();
			ret.SetBaseVector(0, tmp.imaginary);

			tmp = new Quaternion(0, new Vector3D(0, 1, 0));
			tmp = this * tmp * GetConjugate();
			ret.SetBaseVector(1, tmp.imaginary);

			tmp = new Quaternion(0, new Vector3D(0, 0, 1));
			tmp = this * tmp * GetConjugate();
			ret.SetBaseVector(2, tmp.imaginary);

			return ret;
		}

		/// <summary>
		/// Returns an <see cref = "EulerRot"/> that specifies the
		/// same rotation as this quaternion.
		/// </summary>
		/// <returns>The <see cref="EulerRot"/></returns>
		public EulerRot ToEulerRot()
		{
			Quaternion tmpRot = this;
			EulerRot ret = new EulerRot();

			Vector3D forward = new Vector3D(0, 0, 1);
			tmpRot.RotateVector(ref forward);
			ret.yaw = Vector2D.GetAngle(new Vector2D(0, 1), forward.FromTop());

			// remove the yaw from tmpRot
			tmpRot *= Quaternion.YawQuaternion(-ret.yaw);

			forward = new Vector3D(0, 0, 1);
			tmpRot.RotateVector(ref forward);
			ret.pitch = Vector2D.GetAngle(new Vector2D(0, 1), forward.FromRight());

			// remove the pitch from tmpROt
			tmpRot *= Quaternion.PitchQuaternion(-ret.pitch);

			Vector3D right = new Vector3D(1, 0, 0);
			tmpRot.RotateVector(ref right);
			ret.roll = Vector2D.GetAngle(new Vector2D(1, 0), forward.FromFront());

			return ret;
		}

		/// <summary>
		/// Creates a matrix that contains the inverse rotation 
		/// of this quaternion.
		/// </summary>
		/// <returns>A <see cref = "Matrix4"/> that contains the inverse 
        /// rotation of this quaternion.</returns>
		public Matrix4 GetInverseMatrix()
		{
			return GetConjugate().ToMatrix();
		}

		/// <summary>
		/// Applies the rotation of this quaternion to the given vector
		/// </summary>
		/// <param name="v">The vector you want to rotate.</param>
		public void RotateVector(ref Vector3D v)
		{
			Quaternion tmp = new Quaternion(0, v);
			Quaternion res = GetConjugate() * tmp * this;
			v = res.imaginary;
		}

		/// <summary>
		/// Applies the rotation of this quaternion to the input vector, and returns the result.
		/// </summary>
		/// <param name="inVec">The vector to rotate.</param>
		/// <returns>The rotated vector.</returns>
        public Vector3D GetRotatedVector(Vector3D inVec)
		{
			Quaternion tmp = new Quaternion(0, inVec);
			Quaternion res = this * tmp * GetConjugate();
			return res.imaginary;
		}

		/// <summary>
		/// Calculates the dot product of the two quaternions.
		/// </summary>
		/// <param name="a">The first operand.</param>
		/// <param name="b">The second operand</param>
		/// <returns>The result of the dot product operation.</returns>
		public static float DotProduct(Quaternion a, Quaternion b)
		{
			return a.real * b.real + a.imaginary.x * b.imaginary.x +
				a.imaginary.y * b.imaginary.y + a.imaginary.z * b.imaginary.z;
		}

		/// <summary>
		/// Spherically interpolates between the two quaternions. The
		/// resulting quaternion is normalized afterwards, so you don't have to 
		/// do that yourself.
		/// </summary>
		/// <param name="a">The quaternion that corresponds to a t of 0.</param>
		/// <param name="b">The quaternion that corresponds to a t of 1.</param>
		/// <param name="t">The interpolation param.</param>
		/// <returns>The result of the interpolation.</returns>
		public static Quaternion Slerp(Quaternion a, Quaternion b, float t)
		{
			float dot = DotProduct(a, b);
			float angle;

			if (dot > .999f)
			{
				// the two vectors are so close, that a simple linear 
				// interpolation is enough, plus, it's more safe.
				Quaternion ret = a + (b - a) * t;
				ret.Normalize();
				return ret;
			}
			else if (dot < -1)
				angle = (float)Math.PI;
			else
				angle = (float)Math.Acos(dot);

			{				
				float s = (float)Math.Sqrt(angle);

				float p = (float)(Math.Sin(1 - t) * angle) / s;
				float q = (float)(Math.Sin(t) * angle) / s;

				Quaternion ret = a * p + b * q;
				ret.Normalize();
				return ret;
			}
		}

		[Dxml.ParseMethod]
		[ParseMethod]
		public static Quaternion Parse(string s)
		{
			s = s.TrimStart();

			if (s.StartsWith("(deg)") || s.StartsWith("(rad)"))
			{
				EulerRot eulerRot = EulerRot.Parse(s);
				return new Quaternion(eulerRot);
			}
			else if (s.StartsWith("(quat)"))
			{
				s = s.Substring("(quat)".Length);
			}

			string[] components = s.Split(",".ToCharArray());
			if (components.Length != 4)
				throw new FormatException("A quaternion should contain 3 components");

			Quaternion ret = new Quaternion();
			ret.real = float.Parse(components[0], CultureInfo.InvariantCulture);
			ret.imaginary.x = float.Parse(components[1], CultureInfo.InvariantCulture);
			ret.imaginary.y = float.Parse(components[2], CultureInfo.InvariantCulture);
			ret.imaginary.z = float.Parse(components[3], CultureInfo.InvariantCulture);
			return ret;
		}
	}
}
